//
//  motor.c
//  Embedded_System
//
//  Created by Edward Bassan on 3/2/14.
//  Copyright (c) 2014 Edward Bassan. All rights reserved.
//

/*
 pid.c
 Embedded_System
 
 Team Members:
 Edward Bassan
 Kevin Smith
 Irving Estrera
 
 Exercise # 2
 
 Purpose:
 In this exercise you will be given an unknown physical plant simulator for which you need to
 determine the characteristics of the plant and then devise a controller for it. You will be
 using your knowledge and skills from the devices and controls class to implement a control algorithm using the Pi.
 */

#include <stdio.h>
#include <stdlib.h>
#include "pid.h"
#include "global.h"
#include "motor.h"
#include "pid.h"
#include <math.h>
#include <wiringPi.h>
#include <mcp3004.h>
#include <wiringPiI2C.h>
#include <time.h>
#include <unistd.h>
#include <softPwm.h>
#include "sensor.h"
#include <string.h>
#include <errno.h>


//Initializing integral sum
//exponential moving average and
//previous error
int SUM_ERROR = 0;
int PREVIOUS_ERROR = 0;
int EXPONENTIAL_MOVING_AVERAGE = 0;
int DESIRED_VALUE = 0;
int ACTUAL_VALUE = 0;

volatile int encoderR_count = 0;
volatile int encoderL_count = 0;

void ISR_encoderR() {
    encoderR_count++;
    
}



void ISR_encoderL() {
    encoderL_count++;
    
}
int moveRight(MotorPtr this){
    setRightMotorPwr(this, 8, OFF);
    time_t start    = time(0);
    for(;;){
      setLeftMotorSpeed(this, 8, 20, 0);
       time_t end   = time(0);
       time_t timeTaken = end-start;
        
        if(timeTaken == 5000){
            setLeftMotorPwr(this, 8, OFF);
        }
        
    }

    return 0;
}
int moveLeft(MotorPtr this){
    setLeftMotorPwr(this, 8, OFF);
    time_t start    = time(0);
    for(;;){
        setLeftMotorSpeed(this, 8, 20, 0);
        time_t end   = time(0);
        time_t timeTaken = end-start;
        
        if(timeTaken == 5000){
            setRightMotorPwr(this, 8, OFF);
        }
        
    }
    
    return 0;
}
int moveForward(MotorPtr this){
    setLeftDirMotor(this, 8, ON);
    setRightDirMotor(this, 8, ON);
    
    setRightMotorSpeed(this, 8, 20, 0);
    setLeftMotorSpeed(this, 8, 20, 0);
    
    return 0;
}
int moveBack(MotorPtr this){
    
    setLeftDirMotor(this, 8, ON);
    setRightDirMotor(this, 8, ON);
    
    setRightMotorSpeed(this, 8, 20, 0);
    setLeftMotorSpeed(this, 8, 20, 0);
    
    return 0;
}


int setLeftMotorPwr(MotorPtr this, int pin, int state){
    digitalWrite(pin, state);
    return 0;
}
int setRightMotorPwr(MotorPtr this, int pin, int state){
     digitalWrite(pin, state);
    return 0;
}
int setLeftDirMotor(MotorPtr this, int pin, int dir){
    digitalWrite(pin, dir);
    return 0;
}
int setRightDirMotor(MotorPtr this, int pin,  int dir){
    digitalWrite(pin, dir);
    return 0;
}
int setLeftMotorSpeed(MotorPtr this, int pin, int speed, int pid){
    softPwmWrite (pin, speed + pid);
    
    return 0;
}
int setRightMotorSpeed(MotorPtr this, int pin, int speed, int pid){
    softPwmWrite (pin, speed + pid);
    return 0;
}

int avoidObject(MotorPtr this, SensorPtr sensor){
    
    long distance = sensor->getDistanceFromObject(sensor);
    if(distance == 6.0){
        this->moveLeft(this);
    }
    
    return 0;
}

/*
 The main
 */
int main (int argc, char** argv) {

    
    SensorPtr sensor = newSensorConstructor();
    long distance = sensor->getDistanceFromObject(sensor);
    
    
    //Setup for the wiringPi
    if(wiringPiSetup() == -1){
        printf("Fail to setup wiringPi \n");
        exit(1);
    }
    
    pinMode(0, OUTPUT);
    
    if(softPwmCreate (0, 0, 100) == -1){
        printf("Fail to create softPwm \n");
        exit(1);
    }
    
    if(softPwmCreate (7, 0, 100) == -1){
        printf("Fail to create softPwm \n");
        exit(1);
    }
    
    for(;;){
        
        softPwmWrite (0, 15) ;
        softPwmWrite (7, 15) ;
    }
    
    
   
    
    //Variables for the PID control, current error, proportional, derivative and
    // integral
    for(;;){
    int current_error, p_ctrl, i_ctrl, d_ctrl;
    int diff_error;
    int pid_result;
        int kp = 10;
        int ki = 4;
        int kd = 1;
        
    //    int wiringPiISR (1, iINT_EDGE_RISING,  ISR_encoderL);
    //    int wiringPiISR (2, INT_EDGE_RISING,   ISR_encoderR);
    
    //Calculate current error
    current_error = DESIRED_VALUE - ACTUAL_VALUE;
        printf(" current_error %d \n",     current_error);
        
    
    //Proportional control
    p_ctrl = kp * current_error;
    
    //Integral control
    SUM_ERROR = SUM_ERROR + current_error;
    i_ctrl = ki * SUM_ERROR;
                 printf("  i_ctrl %d \n",    i_ctrl);
    
    //Derivative control
    diff_error =  current_error - PREVIOUS_ERROR;
           printf("  diff_error%d \n", diff_error);
    PREVIOUS_ERROR =  current_error;
    d_ctrl = kd * diff_error;
            printf(" d_ctrl %d \n",d_ctrl);
    
    //PID controller output
    pid_result = p_ctrl + i_ctrl + d_ctrl;
        printf(" pid_result %d \n",pid_result);
    
       
         ACTUAL_VALUE = 100 +pid_result;
                printf(" p %d \n", ACTUAL_VALUE);
        
      
        ACTUAL_VALUE = 100 +pid_result;
        printf(" p %d \n", ACTUAL_VALUE);
    
    }
    
    return 0;
}

/*
 int moveSinosodilly(speed, int pid){
 return 0;
 }
 
 */

